#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <malloc.h>
#include <string.h>
#include "BMPInfo.h"
#include "drlsecore.h"

#define PI 3.1415926536
#define byte4_Width(bits) ((bits+31)/32*4)
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define max(a, b) (((a) > (b)) ? (a) : (b))
#define clamp(a, b1, b2) min(max(a, b1), b2);

DRLSECore::DRLSECore()
{
}

void DRLSECore::DoDRLSE()
{
	FILE *in, *out;                    //  *out1, *out2, *out3, *outd1,*outd2, *outd3, *outd4;

	unsigned int originalWidth, originalHeight, ROIpoint[2][2];
	unsigned int initialPointX,  initialPointY;
	unsigned int cWidth, rHeight, c4Width, *imgSize;
	unsigned char *buffer;
	int FileLength, i, j, m,n, bits;
	int fourBsizeImage, skipBytes;
	double *iMAP_1D, *SiMAP, *Ux, *Uy, *g, *f;

	/* Variables for DRLSE */
	char potentialFunction;        // Potential Options
	int **lesionP, nPoints,R0;     // Points assumed as lesion region;

	int iter_inner;
	int iter_outer;
	int potential;

	double timestep;                  // time step
	double mu;        // coefficient of the distance regularization term R(Phi)
	double lambda;        // 5 coefficient of the weighted length term L(Phi)
	double alfa;         // or -3  coefficient of the weighted area term A(Phi)
	double epsilon;     // papramater that specifies the width of the DiracDelta function
	double sigma;        // scale parameter in Gaussian kernel
							 //   G=fspecial('gaussian',30,sigma); % Caussian kernel
	double c0;
	double *ISF, *initialLSF, *Phi, *oldPhi, *newPhi, normSum;

	float normV;


	// size_t result;
	BmpFileHeader_p* hdrFile;
	BmpInfoHeader_p* hdrInfo;
	BmpRGBQUAD_p* rgbQUAD;

	in = fopen("4Image.bmp","rb");
	if (in == NULL) {
		 fprintf(stderr, "Can't open output fileo!\n");
		 exit(1);
	}

 /* **************************************** Read the file information from BMP header ********************************************************* */

	/*  Otain file size: */
	fseek (in , 0 , SEEK_END);
	FileLength = ftell (in);
	rewind (in);

	/* Read the BMP file */
	/* BMP Header File */
	printf(" --------- File Header ------------ \n");
	printf("%ld location 1\n", ftell (in));

	hdrFile = (BmpFileHeader_p*)malloc(sizeof(BmpFileHeader_p));
	fread(hdrFile,sizeof(BmpFileHeader_p),1, in);

	printf("sizeof BmpFileHead: %d\n", sizeof(BmpFileHeader_p));
	printf("sigB: %c\n", hdrFile->sigB);
	printf("sigM: %c\n", hdrFile->sigM);
	printf("File Size:  %d\n", hdrFile->fileSize);
	printf("pixelOffset:  %d\n", hdrFile->pixelOffset);

	/* BMP Header Information */

	printf(" --------- File information ------------ \n");
	printf("%ld location 2\n", ftell (in));

	hdrInfo = (BmpInfoHeader_p*)malloc(sizeof(BmpInfoHeader_p));
	fread(hdrInfo,sizeof(BmpInfoHeader_p),1, in);

	hdrInfo->biSizeImage= hdrInfo->width *hdrInfo->height;

	printf("sizeof BmpInfoHead: %d\n", sizeof(BmpInfoHeader_p));
	printf("image size %d x %d\n", hdrInfo->width ,hdrInfo->height);
	printf("bits per pixel:  %d\n", hdrInfo->bitPix);
	printf("Compression: %d\n", hdrInfo->biCompression);
	printf("Image Size: %d\n", hdrInfo->biSizeImage);
	printf("Color used: %d\n", hdrInfo->biClrUsed);


	/* BMP RGB QUAD Information (RGB Pallete) */
	printf(" --------- File Header ------------ \n");
	printf("%ld location of QUAD Pallate \n", ftell (in));

	rgbQUAD = (BmpRGBQUAD_p*)malloc(256*sizeof(BmpRGBQUAD_p));
	fread(rgbQUAD,256*sizeof(BmpRGBQUAD_p),1, in);

	printf("sizeof BGMQUAD: %d\n", sizeof(BmpRGBQUAD_p));
	printf("RGB Blue %d \n", rgbQUAD[205].rgbBlue);
	printf("RGB Green %d \n", rgbQUAD[205].rgbGreen);
	printf("RGB Red %d \n", rgbQUAD[205].rgbRed);

	/* Read Image Data */
	printf(" --------- Read data ------------ \n");
	printf("%ld location of Raw Data \n", ftell (in));

	fseek(in, hdrFile->pixelOffset, SEEK_SET);   /* Locate the begining address of the image data */
	printf("%ld location of Raw Data offset \n", ftell (in));

	/* Upload the image in a 4 byte unit to a memeory */

	if(hdrInfo->bitPix == 24)
	{
		if (hdrInfo->width % 4 !=0)
		{
			cWidth = hdrInfo->width;
			bits = (hdrInfo->bitPix)*(hdrInfo->width);  /* Make 4-times number of memory unit (4 bytes) */
			c4Width = byte4_Width(bits);    /* Make 4-times number of memory unit (4 bytes) */
			skipBytes = c4Width - 3*hdrInfo->width;
			printf("24 bit test1 \n");
		}
		else
		{
			cWidth = hdrInfo->width;
			c4Width = 3*cWidth;
			skipBytes = 0;
			printf("24 bit (4 bit unit) test1 \n");
		}
	}
	if(hdrInfo->bitPix == 8)
	{
		if(hdrInfo->width % 4 !=0)
		{
			cWidth = hdrInfo->width;
			bits = (hdrInfo->bitPix)*(hdrInfo->width);  /* Make 4-times number of memory unit (4 bytes) */
			c4Width = byte4_Width(bits);    /* Make 4-times number of memory unit (4 bytes) */
			skipBytes = c4Width - hdrInfo->width;
			printf(" 8 bit test1 \n");
			printf("8 bit (4 bit unit) test1 \n");
		}
		else
		{
			cWidth = hdrInfo->width;
			c4Width = cWidth;
			skipBytes = 0;
		}
	}

	rHeight = hdrInfo->height;
	fourBsizeImage = c4Width*rHeight;

	printf("c4Width: %d cWidth: %d  skipBytes %d \n", c4Width, cWidth, skipBytes);
	printf("fourBsizeImage : %d rHeight*cWidth: %d \n", fourBsizeImage, rHeight*cWidth);

	/* Extract the image information from a BMP file and allocate it for image processing */

	buffer = (unsigned char*)malloc(fourBsizeImage*sizeof(unsigned char));

	for (i=rHeight-1; i >= 0 ; i--)
	{
		fread(&buffer[i*c4Width], c4Width, 1, in);     // Read block from a file
	}

	originalWidth = cWidth;
	originalHeight = rHeight;

	/* **************************************** ROI and Initial point setting ********************************************************* */
	// Region Of Interest : Two points are selected, Top-left & bottom-right
	ROIpoint[0][0] = 440;    ROIpoint[0][1] = 240;        // Cropped point, top-left      [][0]: y-axis point [][1]: x-axis point
	ROIpoint[1][0] = 510;    ROIpoint[1][1] = 380;        // Cropped point, bottom-right  [][0]: y-axis point [][1]: x-axis point

	// Initial point for segmentation
	initialPointX =317;                     // Initial points in X-axis given for segmentation (DRLSE algrorithm)
	initialPointY= 480;                     // Initial points in Y-axis given for segmentation (DRLSE algrorithm)

	cWidth =  ROIpoint[1][1] - ROIpoint[0][1];       // new width which is for selected ROI
	rHeight = ROIpoint[1][0] - ROIpoint[0][0];       // new Height which is for selected ROI

	printf(" cwidth:%d  height:%d \n", cWidth, rHeight);

	imgSize = (unsigned int*)malloc(2*sizeof(unsigned int));
	imgSize[0] = cWidth;                                     // ROI (Region Of Interest) Cwidtht;
	imgSize[1] = rHeight;                                    // ROI (Region of Interest) rHeight;
	/* ******************************************************************************************************************************** */


	iMAP_1D = (double*)malloc((rHeight*cWidth)*sizeof(double));
	SiMAP = (double*)malloc((rHeight*cWidth)*sizeof(double));

	Ux = (double*)malloc((rHeight*cWidth)*sizeof(double));
	Uy = (double*)malloc((rHeight*cWidth)*sizeof(double));
	g = (double*)malloc((rHeight*cWidth)*sizeof(double));
	f = (double*)malloc((rHeight*cWidth)*sizeof(double));


	initialLSF = (double*)malloc((rHeight*cWidth)*sizeof(double));
	oldPhi =  (double*)malloc((rHeight*cWidth)*sizeof(double));
	newPhi =  (double*)malloc((rHeight*cWidth)*sizeof(double));
	Phi =  (double*)malloc((rHeight*cWidth)*sizeof(double));
	ISF =  (double*)malloc((rHeight*cWidth)*sizeof(double));

	/* Read the image data for the ROI from the full imgae */
	for(i=0; i<rHeight; i++)
	{
		for(j=0; j<cWidth; j++)
		{                                 // To map a block of cWidth to imageMap
			if (hdrInfo->bitPix == 8)
			{
				iMAP_1D[i*cWidth+j] = (double)buffer[(i+ROIpoint[0][0])*c4Width + j + ROIpoint[0][1] ];   // if an image is 8 bit
			}

			if (hdrInfo->bitPix == 24)
			{
				iMAP_1D[i*cWidth+j] = (double)buffer[(i+ROIpoint[0][0])*c4Width + j*3 + ROIpoint[0][1]*3 ];   // if an image is 24 bit
			}
		}
	}
	/* ****************** Process the ROI image to segment lesions in the teeth using DRLSE (Direct Regularization Level Set Evolution algorithm ************ */
   /** Parameter setting for DRLSE **/
	timestep=1.0;      // time step
	mu=0.1/timestep; // coefficient of the distance regularization term R(Phi)
	iter_inner=5;
	iter_outer=5;
	lambda= 5.0;        // 5 coefficient of the weighted length term L(Phi)
	alfa= -1.0;         // or -3  coefficient of the weighted area term A(Phi)
	epsilon=1.5;     // papramater that specifies the width of the DiracDelta function
	sigma=.8;        // scale parameter in Gaussian kernel
	//   G=fspecial('gaussian',30,sigma); % Caussian kernel

	/* Lesion Point-initial points */
	nPoints=1;
	lesionP= (int**) malloc(2*sizeof(int*));

	for(i=0; i < nPoints; i++)
	{
		lesionP[i] = (int*)malloc(2*sizeof(int));
	}

	/* Generate the initial region R0 as two rectangles */
	R0=5;
	c0=2.0;
	lesionP[0][0]=  initialPointY - ROIpoint[0][0];   // y-direction (height)
	lesionP[0][1]=  initialPointX - ROIpoint[0][1];   // x-direction (width)

	printf(" Initial point in lesion of the teeth  y:%d   x:%d \n",  lesionP[0][0], lesionP[0][1]);


	for (m=0; m<nPoints; m++)
	{
		for (i=0; i<rHeight; i++)
		{
			for (j=0; j<cWidth; j++)
			{
				// Initializ paramters used for DRLSE
				initialLSF[i*cWidth+j] = c0;              //   c0*ones(size(Img));
				if( i >= (lesionP[m][0]-R0+1-1) && i <= (lesionP[m][0]+R0-1))
				{
					if( j >= (lesionP[m][1]-R0-1) && j <= (lesionP[m][1]+R0-1))     // -1 for array order
					{
						initialLSF[i*cWidth+j] = -c0;      // Put -2.0 in the region selected (Ro)
					}
				}
			Phi[i*cWidth+j] = initialLSF[i*cWidth+j];
			}
		}

		/********************** Coherent Filter: To filter the image before applying the DRLSE ********************************* */
		/* Img_smooth=CoherenceFilter(Img,Options);%conv2(Img,G,'same');  */
		printf(" Start Coherent Filter \n");
		Coherent_filter(&iMAP_1D[0], &imgSize[0], &SiMAP[0]) ;
		printf(" Finsih Coherent Filter \n");

		// Smooth image by Gaussiin convolution
		// gradient2Dx(&SiMAP[0], &imgSize[0], Ux);     gradient2Dy(&SiMAP[0], &imgSize[0], Uy);
		GradientM(SiMAP, imgSize, Ux, Uy);

		for (i=0; i<rHeight; i++)
		{
			for (j=0; j<cWidth; j++)
			{
				f[i*cWidth+j]=sqrt(pow(Ux[i*cWidth+j],2.) + pow(Uy[i*cWidth+j],2.)); //  f=sqrt(Ix.^2+Iy.^2);
				g[i*cWidth+j]=  1./(1+pow(f[i*cWidth+j],2.));                        //  g=1./(1+f.^2); Egdge indication function
			}
		}


		/*   Store the image to the buffer back  */
		/* ***************************************************************************** */
		for (i=0; i<rHeight; i++)
		{
			for (j=0; j<cWidth; j++)                        // To write a block of cWidth
			{
				if (hdrInfo->bitPix == 8)
				{
					buffer[(i+ROIpoint[0][0])*c4Width+ j + ROIpoint[0][1]] = (unsigned char)(iMAP_1D[i*cWidth + j]);
				}

				if (hdrInfo->bitPix == 24)
				{
					buffer[(i+ROIpoint[0][0])*c4Width+ j*3 + ROIpoint[0][1]*3] = (unsigned char)(iMAP_1D[i*cWidth + j]);
				}
			}
		}

		potential=1;
		potentialFunction = 'y';

		if (potential == 1)
		{
			potentialFunction = 'x';         // 'single-well';  % use single well potential p1(s)=0.5*(s-1)^2, which is good for region-based model
		}
		else if (potential == 2)
		{
			potentialFunction = 'y';       // 'double-well';  % use double-well potential in Eq. (16), which is good for both edge and region based models
		}
		else
		{
			potentialFunction = 'z';       // 'double-well';  % default choice of potential function
		}

		/* Start level set evolution */
		for (n=1; n<300; n++)
		{
			for (i=0; i<rHeight; i++)
			{
				for (j=0; j<cWidth; j++)
				{
					if (Phi[i*cWidth+j] > -1.0 && (Phi[i*cWidth+j] < 1.0))
					{
						oldPhi[i*cWidth+j] = 1.0;
					}
					else
					{
						oldPhi[i*cWidth+j] = 0.0;
					}
				}  // for (j=0; j<cWidth; j++){
			}  // for (i=0; i<rHeight; i++){

			// Apply drlse algrithm
			drlse_edge(Phi, g, lambda, mu, alfa, epsilon, timestep, iter_inner, potentialFunction, imgSize);

			normSum = 0.0;

			for (i=0; i<rHeight; i++)
			{
				for (j=0; j<cWidth; j++)
				{
					if (Phi[i*cWidth+j] > -1.0 && (Phi[i*cWidth+j] < 1.0))
					{
						newPhi[i*cWidth+j] = 1.0;
					}
					else
					{
						newPhi[i*cWidth+j] = 0.0;
					}

					normSum = normSum + pow((oldPhi[i*cWidth+j] - newPhi[i*cWidth+j]), 2.0);
				}
			}  // for (i=0; i<rHeight; i++)

			normV = sqrt(normSum);

			if (normV < 0.5) {
				printf("Stop iternation! *********************** normSum: %f  n=%d \n",normV, n);
				n=1000;
				break;
			 }
			else{
				printf("Continue iternation! *********************** normSum: %f  n=%d \n",normV, n);
			}

		} // for (n=1; n<2; n++){

		alfa = 0.0;
		drlse_edge(Phi, g, lambda, mu, alfa, epsilon, timestep, iter_inner, potentialFunction, imgSize);

	} // for (m=0; m<nPoints; m++){

	// Draw the segmentation line of the lesion from the processed image.

	contour(iMAP_1D, Phi, imgSize);

	// Send back the segmented ROI image to the original image (Full image)
	for(i=0; i<rHeight; i++)
	{
		for(j=0; j<cWidth; j++)                                 // To map a block of cWidth to imageMap
		{
			if (hdrInfo->bitPix == 8)
			{
				if(iMAP_1D[i*cWidth+j] == 200)
				{
					buffer[(i+ROIpoint[0][0])*c4Width + j + ROIpoint[0][1] ] = 10;  // For 8 bit image
				}


			}

			if (hdrInfo->bitPix == 24)
			{
				if(iMAP_1D[i*cWidth+j] == 200)
				{
					buffer[(i+ROIpoint[0][0])*c4Width + j*3 + ROIpoint[0][1]*3 ] = 10; // For 24 bit image
				}
			}
		}
	}

	free(initialLSF);
	free(Phi);
	free(ISF);

	for(i=0; i < nPoints; i++)
	{
		free (lesionP[i]);
	}

	free(oldPhi);
	free(newPhi);
	fclose(in);

 /* *************************************** Write back the image processed to a BMP file *************************************** */
	out=fopen("Image_out.bmp","wb");

/* Write in the new header */
/* ****************************************************************************** */
	fwrite(hdrFile,sizeof(BmpFileHeader_p),1, out);
	fwrite(hdrInfo,sizeof(BmpInfoHeader_p),1, out);

	if (hdrInfo->bitPix == 8)
	{
	  fwrite(rgbQUAD,256*sizeof(BmpRGBQUAD_p),1, out);
	}

	/* Write in the new image pixel */
	for (i=originalHeight-1; i >= 0; i--)                   // Write a block to a file in the reverse direction
	{
		fwrite(&buffer[i*c4Width],c4Width,1,out);
	}

	free(hdrFile);
	free(hdrInfo);
	free(rgbQUAD);
	free(buffer);
	free(iMAP_1D);
	free(SiMAP);
	free(imgSize);
	free(Ux);
	free(Uy);
	free(g);
	free(f);

	printf("End of the process :  \n");
	fclose(out);
}

void DRLSECore::Anisotropic_step2D(double *iMAP_1D, unsigned int *imgSize, Options_p *Options, double *SiMAP, int k)
{
	int i=0;
	int kernel_size;
	unsigned int npixels2 = 0;
	double sigma0, rho0;
	/* *iMAP_1D : Original image */
	/* *SiMAP : Gaussian Filtering image */
	double *Ux, *Uy;                 /* Gradients of smoothed image */
	double *Jxx, *Jxy, *Jyy;  /* Structure tensor */
	double *V[6];                    /* Eigen vector & value,  mu1, mu2 : Matrices with eigen values, v1x, v1y, v2x, v2y : Matrices with the eigen vectors  */
	double *Dxx, *Dxy, *Dyy;         /* Diffusion tensor */
	double *gradA;

	npixels2 = imgSize[0]*imgSize[1];
	kernel_size=1*4;                         // sigma............
	sigma0 = Options->sigma;
	rho0 = Options->rho;

	GaussianFiltering2D_double(&iMAP_1D[0], &imgSize[0], sigma0, kernel_size, SiMAP);
	Ux = (double*)malloc(npixels2*sizeof(double));
	Uy = (double*)malloc(npixels2*sizeof(double));

	/* Calculate the gradients  */
	switch (Options->scheme)
	{
	case 'i' :
		gradient2Dx(&SiMAP[0], &imgSize[0], Uy);              // checkout Uy
		gradient2Dy(&SiMAP[0], &imgSize[0], Ux);              // checkout Ux
		break;
	case 's' :
		GradientM(SiMAP, imgSize, Uy, Ux);                  // Check Uy Ux order
		break;
	default:
		 printf("Coherence Filter: Unknown filter option for gradient \n" );
	}
	// Compute the 2D structure tensors J of the image
//	printf("Test : diffusion 2D_rotation invariant \n" );
	Jxx=(double *)malloc(npixels2*sizeof(double));   // Jxx

	if (Jxx == NULL) printf("Jxx: Out of Memory \n");

	Jxy =(double *)malloc(npixels2*sizeof(double));   // Jxy

	if (Jxy == NULL) printf("Jxy: Out of Memory \n");

	Jyy =(double *)malloc(npixels2*sizeof(double));   // Jyy

	if (Jyy == NULL) printf("Jyy: Out of Memory \n");

	kernel_size = 6*rho0;
	StructureTensor2D( Ux, Uy, Jxx,Jxy,Jyy, imgSize, rho0, kernel_size);

	// Compute the eigenvectors and values of the strucure tensors, v1 and v2, mu1 and mu2
	V[0]=(double *)malloc(npixels2*sizeof(double));   // V[0] = Mu1

	if (V[0] == NULL) printf("V[0]: Out of Memory \n");

	V[1]=(double *)malloc(npixels2*sizeof(double));   // V[1] = Mu2

	if (V[1] == NULL) printf("V[1]Jxy: Out of Memory \n");

	V[2]=(double *)malloc(npixels2*sizeof(double));   // V[2] = v2x

	if (V[2] == NULL) printf("V[2]Jyy: Out of Memory \n");

	V[3]=(double *)malloc(npixels2*sizeof(double));   // V[3] = v2y

	if (V[3] == NULL) printf("V[3]: Out of Memory \n");

	V[4]=(double *)malloc(npixels2*sizeof(double));   // V[4] = v1x

	if (V[4] == NULL) printf("V[4]Jxy: Out of Memory \n");

	V[5]=(double *)malloc(npixels2*sizeof(double));   // V[5] = v1y

	if (V[5] == NULL) printf("V[5]Jyy: Out of Memory \n");

	EigenVector2D(Jxx,Jxy,Jyy, imgSize, V);

	free(Jxx);
	free(Jxy);
	free(Jyy);

	/* Construct the edge preserving diffusion tensors D = [Dxx,Dxy;Dxy,Dyy]  */
	gradA=(double *)malloc(npixels2*sizeof(double));

	if (gradA == NULL) printf("gradA: Out of Memory \n");

	for (i=0; i<npixels2; i++)
	{
		gradA[i] = pow(Ux[i],2) + pow(Uy[i],2);
		SiMAP[i] = iMAP_1D[i];  // Calculation for Diffusion_scheme_2D_standard  come back to usigma -> u
	}

	free(Ux);
	free(Uy);

	Dxx=(double *)malloc(npixels2*sizeof(double));

	if (Dxx == NULL) printf("Dxx: Out of Memory \n");

	Dxy =(double *)malloc(npixels2*sizeof(double));

	if (Dxy == NULL) printf("Dxy: Out of Memory \n");

	Dyy =(double *)malloc(npixels2*sizeof(double));

	if (Dyy == NULL) printf("Dyy: Out of Memory \n");

	/* [Dxx,Dxy,Dyy]=ConstructDiffusionTensor2D(mu1,mu2,v1x,v1y,v2x,v2y,gradA,Options);  */
	ConstructDiffusionTensor2D(V, Dxx, Dxy, Dyy, gradA, imgSize, Options);

	free(gradA);
	for (i=0;i<6;i++)
	{
		free(V[i]);
	}

	/* Do the image diffusion  */
	switch (Options->scheme)  //Options->scheme
	{
	case 'r' :
		printf("Test : diffusion 2D_rotation invariant \n" );
		// u=Diffusion_scheme_2D_rotation_invariant(u,Dxx,Dxy,Dyy,Options.dt);
		break;
	case 's' :
		// u=Diffusion_scheme_2D_standard(u,Dxx,Dxy,Dyy,Options.dt);
		Diffusion_scheme_2D_standard(SiMAP, Dxx, Dxy, Dyy, imgSize, Options);
		break;
	case 'i' :
		// u=Diffusion_scheme_2D_standard(u,Dxx,Dxy,Dyy,Options.dt);
		Diffusion_scheme_2D_implicit(SiMAP, Dxx, Dxy, Dyy, imgSize, Options);
		break;
	default:
		printf("Coherence Filter: Unknown filter option for gradient........ \n" );
	}

	free(Dxx);
	free(Dxy);
	free(Dyy);
}

void DRLSECore::Coherent_filter(double *iMAP_1D, unsigned int *imgSize, double *SiMAP)
{
	int i,j,k;
	double *iMAP_1D_Temp;
	double dt_max ,t;

	Options_p *Options;
	Options = (Options_p*)malloc(sizeof (Options_p));

	Options->verbose  = 'i';   //iter
	Options->scheme  = 'i';
	Options->T = 25;  //25
	Options->dt = 0.0;
	Options->rho = 2.0;
	Options->sigma = 1.0;
	Options->eigenmode = 0;
	Options->C = 1e-10;
	Options->m = 1.0;
	Options->alpha = 0.001;
	Options->lambda_e  =0.02;
	Options->lambda_c  =0.02;
	Options->lambda_h  =0.5;

	printf("111 Coherence Filter: Unknown option  %f \n", Options->dt );

	if (Options->dt == 0.0)
	{
		switch (Options->scheme)
		{
		case 'i' :
			Options->dt = 0.15;
			break;
		case 's' :
			Options->dt = 0.15;
			break;
		default:
			printf("Coherence Filter: Unknown option\n" );
		}
	}

	/* Set up the deffusion time */
	dt_max = Options->dt;         /* Initialization */
	t = 0;                        /* Initialization */

	/* Show information on the filtering method  */

	printf("222 Coherence Filter: Unknown option\n" );

	iMAP_1D_Temp=(double *)malloc(imgSize[0]*imgSize[1]*sizeof(double));

	if (iMAP_1D_Temp == NULL) printf("iMAP_1D_Temp: Out of Memory \n");

	for (i=0; i < imgSize[0]*imgSize[1];i++)
	{
			 iMAP_1D_Temp[i] = iMAP_1D[i];
	}

	/*  Anisotropic diffusion main loop  */
	j=0;k=0;

	while (t < (Options->T - 0.001))
	{
		/* Update time, adjust last time step to exactly finish at the wanted diffusion time */
		Options->dt = min(dt_max,Options->T-t);
		t = t + Options->dt;
		k=k+1;
		/*  Do a diffusion process */
		Anisotropic_step2D(&iMAP_1D_Temp[0], &imgSize[0], Options, &SiMAP[0],k);

		for (i=0; i < imgSize[0]*imgSize[1];i++)
		{
			  iMAP_1D_Temp[i] = SiMAP[i];
		}
	} // while

	free(Options);
	free(iMAP_1D_Temp);
}

void DRLSECore::imfilter1D_double(double *iMAP_1D, unsigned int *imgSize, double *H, int lengthH, double *jrow)
{
	int x, i, index, offset;
	int b2, offset2;
	unsigned int lengthI = imgSize[0];

	if(lengthI==1)
	{
		jrow[0]=iMAP_1D[0];
	}
	else
	{
		offset=(int)((lengthH-1)/2);

		for(x=0; x<min(offset,lengthI); x++)
		{
			jrow[x]=0;
			b2=lengthI-1; offset2=x-offset;
			for(i=0; i<lengthH; i++)
			{
				index=clamp(i+offset2, 0, b2); jrow[x]+=iMAP_1D[index]*H[i];
			}
		}

		for(x=offset; x<(lengthI-offset); x++)
		{
			jrow[x]=0;
			b2=lengthI-1; offset2=x-offset;
			for(i=0; i<lengthH; i++)
			{
				index=i+offset2;
				jrow[x]+=iMAP_1D[index]*H[i];
			}
		}

		b2=lengthI-1;

		for(x=max(lengthI-offset,offset); x<lengthI; x++)
		{
			jrow[x]=0;
			offset2=x-offset;

			for(i=0; i<lengthH; i++)
			{
				index=clamp(i+offset2, 0, b2); jrow[x]+=iMAP_1D[index]*H[i];
			}
		}
	}
}

void DRLSECore::imfilter2D_double(double *iMAP_1D, unsigned int *imgSize, double *H, int lengthH, double *jMAP_1D)
{
	int y, x, i, y2;
	double *Irow, *Jrow;
	int index=0, line=0;
	double *RCache;
	int *nCache;
	int hks, offset, offset2;

	RCache=(double *)malloc(lengthH*imgSize[0]*sizeof(double));

	for(i=0; i<lengthH*imgSize[0]; i++)
	{
		RCache[i]=0;
	}

	nCache=(int *)malloc(lengthH*sizeof(int));

	for(i=0; i<lengthH; i++)
	{
		nCache[i]=0;
	}

	hks=((lengthH-1)/2);

	for(y=0; y<min(hks,imgSize[1]); y++)
	{
		Irow=&iMAP_1D[index];
		Jrow=&RCache[line*imgSize[0]];
		imfilter1D_double(Irow, &imgSize[0], H, lengthH, Jrow);
		index+=imgSize[0];

		if(y!=(imgSize[1]-1))
		{
			line++; if(line>(lengthH-1)) { line=0; }
		}

		for(i=0; i<(lengthH-1); i++)
		{
			nCache[i]=nCache[i+1];
		}
		nCache[lengthH-1]=line;
	}

	for(y2=y; y2<hks; y2++)
	{
		for(i=0; i<(lengthH-1); i++)
		{
			nCache[i]=nCache[i+1];
		}

		nCache[lengthH-1]=line;
	}

	for(y=hks; y<(imgSize[1]-1); y++)
	{
		Irow=&iMAP_1D[index];
		Jrow=&RCache[line*imgSize[0]];
		imfilter1D_double(Irow, &imgSize[0], H, lengthH, Jrow);
		offset=(y-hks)*imgSize[0];
		offset2=nCache[0]*imgSize[0];
		for(x=0; x<imgSize[0]; x++)
		{
			jMAP_1D[offset+x]=RCache[offset2+x]*H[0];
		}
		for(i=1; i<lengthH; i++)
		{
			offset2=nCache[i]*imgSize[0];
			for(x=0; x<imgSize[0]; x++)
			{
				jMAP_1D[offset+x]+=RCache[offset2+x]*H[i];
			}
		}

		index+=imgSize[0];
		line++;

		if(line>(lengthH-1)) line=0;

		for(i=0; i<(lengthH-1); i++)
		{
			nCache[i]=nCache[i+1];
		}

		nCache[lengthH-1]=line;
	}

	for(y=max(imgSize[1]-1,hks); y<imgSize[1]; y++)
	{
		Irow=&iMAP_1D[index];
		Jrow=&RCache[line*imgSize[0]];
		offset=(y-hks)*imgSize[0]; offset2=nCache[0]*imgSize[0];

		for(x=0; x<imgSize[0]; x++)
		{
		   jMAP_1D[offset+x]=RCache[offset2+x]*H[0];
		}

		for(i=1; i<lengthH; i++)
		{
			offset2=nCache[i]*imgSize[0];

			for(x=0; x<imgSize[0]; x++)
			{
				jMAP_1D[offset+x]+=RCache[offset2+x]*H[i];
			}
		}

		index+=imgSize[0];

		for(i=0; i<(lengthH-1); i++)
		{
			nCache[i]=nCache[i+1];
		}

		nCache[lengthH-1]=line;
	}

	for(y=max(imgSize[1],hks); y<(imgSize[1]+hks); y++)
	{
		offset=(y-hks)*imgSize[0]; offset2=nCache[0]*imgSize[0];
		for(x=0; x<imgSize[0]; x++)
		{
			jMAP_1D[offset+x]=RCache[offset2+x]*H[0];
		}

		for(i=1; i<lengthH; i++)
		{
			offset2=nCache[i]*imgSize[0];
			for(x=0; x<imgSize[0]; x++)
			{
				jMAP_1D[offset+x]+=RCache[offset2+x]*H[i];
			}
		}

		index+=imgSize[0];

		for(i=0; i<(lengthH-1); i++)
		{
			nCache[i]=nCache[i+1];
		}

		nCache[lengthH-1]=line;
	}

	free(RCache);
	free(nCache);
}

void DRLSECore::gradient2Dx(double *SiMAP, unsigned int *imgSize, double *Ux)
{
	int x,y;
	int yp, yn;
	int indexn, indexc, indexp;
	double *Irow;
	const double smoothfilter[2]={0.093750,0.31250};

	Irow=(double *)malloc(imgSize[0]*sizeof(double));

	for(y=0; y<imgSize[1]; y++)
	{
	  /* Smooth in y - direction */
		yn=max(y-1,0);
		yp=min(y+1,imgSize[1]-1);
		indexn=yn*imgSize[0]; indexc=y*imgSize[0]; indexp=yp*imgSize[0];
		for(x=0; x<imgSize[0]; x++)
		{
			Irow[x] =smoothfilter[0]*SiMAP[indexn+x];
			Irow[x]+=smoothfilter[1]*SiMAP[indexc+x];
			Irow[x]+=smoothfilter[0]*SiMAP[indexp+x];
		}
	  /* Gradient in x - direction */
		Ux[indexc]=2*(Irow[1]-Irow[0]);

		for(x=1; x<(imgSize[0]-1); x++)
		{
			Ux[indexc+x]=Irow[x+1]-Irow[x-1];
		}

		Ux[indexc+imgSize[0]-1]=2*(Irow[imgSize[0]-1]-Irow[imgSize[0]-2]);
	}

	free(Irow);
}

void DRLSECore::gradient2Dy(double *SiMAP, unsigned int *imgSize, double *Uy)
{
	int x,y;
	int indexy, indexyy, indexn, indexc, indexp;
	double *Irow;
	int row_select=0, row_select_p1=0, row_select_p2=0;
	const double smoothfilter[2]={0.093750,0.31250};
	/*const double smoothfilter[2]={0.07322330470336310700,0.35355339059327379000}; */

	Irow=(double *)malloc(imgSize[0]*4*sizeof(double));

	for(y=0; y<imgSize[1]; y++)
	{
		/* Smooth in x - direction */
		indexyy=y*imgSize[0];
		indexy=row_select*imgSize[0];
		Irow[indexy]=SiMAP[indexyy]*(smoothfilter[0]+smoothfilter[1])+SiMAP[indexyy+1]*smoothfilter[0];

		for(x=1; x<(imgSize[0]-1); x++)
		{
			indexy++; indexyy++;
			Irow[indexy] =smoothfilter[0]*SiMAP[indexyy-1];
			Irow[indexy]+=smoothfilter[1]*SiMAP[indexyy];
			Irow[indexy]+=smoothfilter[0]*SiMAP[indexyy+1];
		}

		indexy++; indexyy++;
		Irow[indexy]=SiMAP[indexyy]*(smoothfilter[0]+smoothfilter[1])+SiMAP[indexyy-1]*smoothfilter[0];

		/* Gradient in y - direction */
		if(y==1) /* Forward */
		{
			for(x=0; x<imgSize[0]; x++)
			{
				Uy[x]=2*(Irow[x+imgSize[0]]-Irow[x]);
			}
		}
		else if(y>1) /* Central */
		{
			indexn=row_select_p2*imgSize[0]; indexp=row_select*imgSize[0]; indexyy=(y-1)*imgSize[0];
			for(x=0; x<imgSize[0]; x++)
			{
				Uy[x+indexyy]=Irow[x+indexp]-Irow[x+indexn];
			}
		}

		if(y==(imgSize[1]-1)) /* Backward */
		{
			indexn=row_select_p1*imgSize[0]; indexc=row_select*imgSize[0]; indexyy=(imgSize[1]-1)*imgSize[0];

			for(x=0; x<imgSize[0]; x++)
			{
				Uy[x+indexyy]=2*(Irow[x+indexc]-Irow[x+indexn]);
			}
		}

		row_select_p2=row_select_p1; row_select_p1=row_select; row_select++; if(row_select>3) { row_select=0; }
	}

	free(Irow);
}

void DRLSECore::StructureTensor2D(double *Ux, double *Uy, double *Jxx, double *Jxy, double *Jyy, unsigned int *imgSize, double rho0, int kernel_size)
{
	unsigned int npixels2;
	int i,j;
	double *J0xx, *J0xy, *J0yy;      /* Structure tensor */

	npixels2 = imgSize[0]*imgSize[1];

	J0xx=(double *)malloc(npixels2*sizeof(double));

	if (J0xx == NULL) printf("Jxx: Out of Memory \n");

	J0xy=(double *)malloc(npixels2*sizeof(double));

	if (J0xy == NULL) printf("Jxy: Out of Memory \n");

	J0yy=(double *)malloc(npixels2*sizeof(double));

	if (J0yy == NULL) printf("Jyy: Out of Memory \n");

	/* J(grad u_sigma) */
	for(i=0; i<npixels2; i++)
	{
		J0xx[i] = Ux[i]*Ux[i];
		J0xy[i] = Ux[i]*Uy[i];
		J0yy[i] = Uy[i]*Uy[i];
	}

	/* Do the gaussian smoothing */
	kernel_size = 6;  //rho0*6
	GaussianFiltering2D_double(J0xx, &imgSize[0], rho0, kernel_size, Jxx);
	free(J0xx);
	GaussianFiltering2D_double(J0xy, &imgSize[0], rho0, kernel_size, Jxy);
	free(J0xy);
	GaussianFiltering2D_double(J0yy, &imgSize[0], rho0, kernel_size, Jyy);
	free(J0yy);
}

void DRLSECore::EigenVector2D(double *Jxx, double *Jxy, double *Jyy, unsigned int *imgSize, double **V)
{
	int i;
	double mag, tmp, tmp1, tmp2, tmp3;
	unsigned npixels2;

	npixels2 = imgSize[0]*imgSize[1];

	/* Compute the eigenvectors of J, v1 and v2 (V[0] = Mu1  V[1] = Mu2 V[2] = v2x V[3] = v2y V[4] = v1x V[5] = v1y) */
	for (i=0; i<npixels2; i++)
	{
		tmp = sqrt( pow(Jxx[i] - Jyy[i], 2.) + 4*pow(Jxy[i],2.));  // tmp = sqrt(pow2(Jxx - Jyy) + 4*pow2(Jxy));
		V[2][i] = 2.*Jxy[i]; 	                                    // v2x =  2*Jxy;
		V[3][i] = Jyy[i] - Jxx[i] + tmp;                             // v2y  = Jyy - Jxx + tmp;

		/* Normalize */
		mag = sqrt(pow(V[2][i],2.) + pow(V[3][i],2.));

		if(mag != 0.0)
		{
			V[2][i] = V[2][i]/mag;              // v2x = v2x/mag;
			V[3][i] = V[3][i]/mag;              // v2y = v2y/mag;
		}

		/* The eigenvectors are orthogonal */
		V[4][i] = -V[3][i];                     // v1x = -v2y;
		V[5][i] =  V[2][i];                     // v1y = v2x;

		/* Compute the eigenvalues */
		V[0][i] = 0.5*(Jxx[i] + Jyy[i] + tmp);      //  mu1 = 0.5*(Jxx + Jyy + tmp);
		V[1][i] = 0.5*(Jxx[i] + Jyy[i] - tmp);      //  mu2 = 0.5*(Jxx + Jyy - tmp);

		/* Make output structure */
		if(fabs(V[0][i]) > fabs(V[1][i]) ) 	             // if(absd(mu1)>absd(mu2))
		{
			tmp1  = V[1][i];
			V[1][i] = V[0][i];
			V[0][i] = tmp1;
			tmp2  = V[2][i];
			V[2][i] = V[4][i];
			V[4][i] = tmp2;
			tmp3  = V[3][i];
			V[3][i] = V[5][i];
			V[5][i] = tmp3;
		}
	}
}

void DRLSECore::ConstructDiffusionTensor2D(double **V, double *Dxx, double *Dxy, double *Dyy, double *gradA, unsigned int *imgSize, Options_p *Options)
{
	unsigned int npixels2;
	int i,j;

	double diff, xiff;
	double lambda1, lambda2;
	double lambdac1, lambdac2, epsilon, lambdae1, lambdae2;
	npixels2 = imgSize[0]*imgSize[1];

	/* Scaling of diffusion tensors */
	if ((Options->eigenmode==0.0)||(Options->eigenmode==1.0)){ // 0 or 1 : Weickerts equation line like kernal
		for (i=0; i<npixels2; i++)
		{
			diff = V[0][i]-V[1][i];             // Implicit if mu1 == mu2 then lambda1=alpha

			if (diff > -1e-15 && diff < 1e-15) diff = 1e-15;

			lambda1 = Options->alpha + (1.0 - Options->alpha)*exp(-Options->C/pow(diff,2*Options->m));
			lambda2 = Options->alpha;
			Dxx[i] = lambda1*pow(V[2][i], 2)   + lambda2*pow(V[4][i],2);
			Dxy[i] = lambda1*V[2][i]*V[3][i] + lambda2*V[4][i]*V[5][i];
			Dyy[i] = lambda1*pow(V[3][i], 2)   + lambda2*pow(V[5][i],2);
		}
	}
	else if (Options->eigenmode==2.0)      // 2 : Edge enhancing diffusion (EED)
	{
		for (i=0; i<npixels2; i++)
		{
			lambda2 = 1. - exp(-3.31488/pow(gradA[i]/pow(Options->lambda_e,2),4));

			if (gradA[i]<1e-15) lambda2 = 1.;

			lambda1 = 1.;
			Dxx[i] = lambda1*pow(V[2][i], 2)   + lambda2*pow(V[4][i],2);
			Dxy[i] = lambda1*V[2][i]*V[3][i] + lambda2*V[4][i]*V[5][i];
			Dyy[i] = lambda1*pow(V[3][i], 2)   + lambda2*pow(V[5][i],2);
		}
	}
	else if (Options->eigenmode==3.0)      // 3 : Coherence-enhancing diffusion (EED)
	{
		for (i=0; i<npixels2; i++)
		{
			diff = V[0][i]-V[1][i];             // Implicit if mu1 == mu2 then lambda1=alpha

			if (diff > -1e-15 && diff < 1e-15) diff = 1e-15;

			lambda1 = Options->alpha + (1.0 - Options->alpha)*exp(-Options->C/pow(diff,2*Options->m));
			lambda2 = Options->alpha;
			Dxx[i] = lambda1*pow(V[2][i], 2)   + lambda2*pow(V[4][i],2);
			Dxy[i] = lambda1*V[2][i]*V[3][i] + lambda2*V[4][i]*V[5][i];
			Dyy[i] = lambda1*pow(V[3][i], 2) + lambda2*pow(V[5][i],2);
		}
	}
	else if (Options->eigenmode==4.0)      // 4 : Hybrid Diffusion with Continous Switch
	{
		for (i=0; i<npixels2; i++)
		{
			// gradA=mean(gradA,3); for 3 dimensions
			lambdae2 = 1. - exp(-3.31488/pow(gradA[i]/pow(Options->lambda_e,2),4));

			if (gradA[i]<1e-15) lambdae2 = 1.;

			lambdae1 = 1.;
			diff = V[0][i]-V[1][i];             // Implicit if mu1 == mu2 then lambda1=alpha

			if (diff > -1e-15 && diff < 1e-15) diff = 1e-15;

			lambdac1 = Options->alpha + (1.0 - Options->alpha)*exp(-Options->C/pow(diff,2*Options->m));
			lambdac2 = Options->alpha;
			xiff = diff;
			diff = 2.0*pow(Options->lambda_h, 4);
			epsilon = exp(V[0][i]*pow(Options->lambda_h,2)*(xiff-abs(xiff))/diff);
			lambda1 = (1. -epsilon)*lambdac1 + epsilon*lambdae1;
			lambda2 = (1. -epsilon)*lambdac2 + epsilon*lambdae2;
			Dxx[i] = lambda1*pow(V[2][i], 2)   + lambda2*pow(V[4][i],2);
			Dxy[i] = lambda1*V[2][i]*V[3][i] + lambda2*V[4][i]*V[5][i];
			Dyy[i] = lambda1*pow(V[3][i], 2)   + lambda2*pow(V[5][i],2);
		}
	}
}

void DRLSECore::Diffusion_scheme_2D_standard(double *SiMAP, double *Dxx, double *Dxy, double *Dyy, unsigned int *imgSize, Options_p *Options)
{
	int i,j;
	int N1, N2, N3=1;
	unsigned int npixels2;
	double *anx, *apx;
	double *bnx, *bny, *bpx, *bpy;
	double *cny, *cpy;
	double *upy, *unx, *upx, *uny;
	double *unxpy, *upxpy, *upxny, *unxny;
	double *A1, *A2, *A3, *A4, *A6, *A7, *A8, *A9;

	npixels2 = imgSize[0]*imgSize[1];
	N1 = imgSize[1];
	N2 = imgSize[0];
	anx=(double *)malloc(npixels2*sizeof(double));

	if (anx == NULL) printf("gradA: Out of Memory \n");

	apx=(double *)malloc(npixels2*sizeof(double));

	if (apx == NULL) printf("gradA: Out of Memory \n");

	bnx=(double *)malloc(npixels2*sizeof(double));

	if (bnx == NULL) printf("gradA: Out of Memory \n");

	bny=(double *)malloc(npixels2*sizeof(double));

	if (bny == NULL) printf("gradA: Out of Memory \n");

	bpx=(double *)malloc(npixels2*sizeof(double));

	if (bpx == NULL) printf("gradA: Out of Memory \n");

	bpy=(double *)malloc(npixels2*sizeof(double));

	if (bpy == NULL) printf("gradA: Out of Memory \n");

	cny=(double *)malloc(npixels2*sizeof(double));

	if (cny == NULL) printf("gradA: Out of Memory \n");

	cpy=(double *)malloc(npixels2*sizeof(double));

	if (cpy == NULL) printf("gradA: Out of Memory \n");

	unx=(double *)malloc(npixels2*sizeof(double));

	if (unx == NULL) printf("gradA: Out of Memory \n");

	upx=(double *)malloc(npixels2*sizeof(double));

	if (upx == NULL) printf("gradA: Out of Memory \n");

	uny=(double *)malloc(npixels2*sizeof(double));

	if (uny == NULL) printf("gradA: Out of Memory \n");

	upy=(double *)malloc(npixels2*sizeof(double));

	if (upy == NULL) printf("gradA: Out of Memory \n");

	unxny=(double *)malloc(npixels2*sizeof(double));

	if (unxny == NULL) printf("gradA: Out of Memory \n");

	upxpy=(double *)malloc(npixels2*sizeof(double));

	if (upxpy == NULL) printf("gradA: Out of Memory \n");

	unxpy=(double *)malloc(npixels2*sizeof(double));

	if (unxpy == NULL) printf("gradA: Out of Memory \n");

	upxny=(double *)malloc(npixels2*sizeof(double));

	if (upxny == NULL) printf("gradA: Out of Memory \n");

	for (i=0; i<N1 ;i++)
	{
		for (j=0; j<N2 ;j++)
		{
			if(i==0)
			{
				anx[j] = Dxx[j];
				bnx[j] = Dxy[j];
				unx[j] = SiMAP[j];
				apx[(N1-1)*N2 + j] = Dxx[(N1-1)*N2 + j];
				bpx[(N1-1)*N2 + j] = Dxy[(N1-1)*N2 + j];
				upx[(N1-1)*N2 + j] = SiMAP[(N1-1)*N2 + j];
			}
			else
			{
				anx[N2*i + j] = Dxx[N2*(i-1) + j];
				bnx[N2*i + j] = Dxy[N2*(i-1) + j];
				unx[N2*i + j] = SiMAP[N2*(i-1) + j];
				apx[N2*(i-1) + j] = Dxx[N2*i + j];
				bpx[N2*(i-1) + j] = Dxy[N2*i + j];
				upx[N2*(i-1) + j] = SiMAP[N2*i + j];
			}
		}
	}

	for (i=0; i<N2 ;i++)
	{
		for (j=0; j<N1 ;j++)
		{
			if(i==0)
			{
				bny[N2*j] = Dxy[N2*j];
				cny[N2*j] = Dyy[N2*j];
				uny[N2*j] = SiMAP[N2*j];
				unxny[N2*j] = unx[N2*j];                    //1 unxny: unx->uny
				upxny[N2*j] = upx[N2*j];                    //2 upxny: upx->uny
				bpy[N2*j+ N2-1] = Dxy[N2*j+N2-1];
				cpy[N2*j+ N2-1] = Dyy[N2*j+N2-1];
				upy[N2*j+ N2-1] = SiMAP[N2*j+N2-1];
				unxpy[N2*j+ N2-1] =unx[N2*j+N2-1];           //3 unxpy: unx->upy
				upxpy[N2*j+ N2-1] =upx[N2*j+N2-1];           //4 upxpy: upx->upy
			}
			else
			{
				bny[N2*j + i] = Dxy[N2*j + i-1];
				cny[N2*j + i] = Dyy[N2*j + i-1];
				uny[N2*j + i] = SiMAP[N2*j + i-1];
				unxny[N2*j + i] = unx[N2*j + i-1];          //1 unxny: unx->uny
				upxny[N2*j + i] = upx[N2*j + i-1];          //2 upxny: upx->uny
				bpy[N2*j + i-1] = Dxy[N2*j + i];
				cpy[N2*j + i-1] = Dyy[N2*j + i];
				upy[N2*j + i-1] = SiMAP[N2*j + i];
				unxpy[N2*j + i-1] = unx[N2*j + i];          //3 unxpy: unx->upy
				upxpy[N2*j + i-1] =upx[N2*j + i];           //4 upxpy: upx->upy
			}
		}
	}

	A1=(double *)malloc(npixels2*sizeof(double));

	if (A1 == NULL) printf("A1: Out of Memory \n");

	A2=(double *)malloc(npixels2*sizeof(double));

	if (A2 == NULL) printf("A2: Out of Memory \n");

	A3=(double *)malloc(npixels2*sizeof(double));

	if (A3 == NULL) printf("A3: Out of Memory \n");

	A4=(double *)malloc(npixels2*sizeof(double));

	if (A4 == NULL) printf("A4: Out of Memory \n");

	A6=(double *)malloc(npixels2*sizeof(double));

	if (A6 == NULL) printf("A6: Out of Memory \n");

	A7=(double *)malloc(npixels2*sizeof(double));

	if (A7 == NULL) printf("A7: Out of Memory \n");

	A8=(double *)malloc(npixels2*sizeof(double));

	if (A8 == NULL) printf("A8: Out of Memory \n");

	A9=(double *)malloc(npixels2*sizeof(double));

	if (A9 == NULL) printf("A9: Out of Memory \n");

	for (i=0; i<N1 ; i++)
	{
		for (j=0; j<N2 ; j++)                                  //     Stencil Weights
		{
			A1[i*N2 + j] = (1/4.)*(bnx[i*N2 + j] - bpy[i*N2 + j]);  // A1=(1/4)*(b(nx,:)-b(:,py));
			A2[i*N2 + j] = (1/2.)*(cpy[i*N2 + j] + Dyy[i*N2 + j]);    // A2=(1/2)*(c(:,py)+c);
			A3[i*N2 + j] = (1/4.)*(bpx[i*N2 + j] + bpy[i*N2 + j]);    // A3=(1/4)*(b(px,:)+b(:,py))
			A4[i*N2 + j] = (1/2.)*(anx[i*N2 + j] + Dxx[i*N2 + j]);    // A4=(1/2)*(a(nx,:)+a)
			A6[i*N2 + j] = (1/2.)*(apx[i*N2 + j] + Dxx[i*N2 + j]);    // A6=(1/2)*(a(px,:)+a);
			A7[i*N2 + j] = (1/4.)*(bnx[i*N2 + j] + bny[i*N2 + j]);  // A7=(1/4)*(b(nx,:)+b(:,ny));
			A8[i*N2 + j] = (1/2.)*(cny[i*N2 + j] + Dyy[i*N2 + j]);    // A8=(1/2)*(c(:,ny)+c);
			A9[i*N2 + j] = (1/4.)*(bpx[i*N2 + j] - bny[i*N2 + j]);    // A9=(1/4)*(b(px,:)-b(:,ny));
		}
	}

	for (i=0; i<N1 ; i++)
	{
		for (j=0; j<N2 ; j++)                                  //     Stencil Weights
		{
			SiMAP[i*N2 + j] = SiMAP[i*N2 + j] +
					Options->dt *(A1[i*N2 + j]*(unxpy[i*N2 + j] - SiMAP[i*N2 + j])+
					A2[i*N2 + j]*(upy[i*N2 + j]   - SiMAP[i*N2 + j])+
					A3[i*N2 + j]*(upxpy[i*N2 + j] - SiMAP[i*N2 + j])+
					A4[i*N2 + j]*(unx[i*N2 + j]   - SiMAP[i*N2 + j])+
					A6[i*N2 + j]*(upx[i*N2 + j]   - SiMAP[i*N2 + j])+
					A7[i*N2 + j]*(unxny[i*N2 + j] - SiMAP[i*N2 + j])+
					A8[i*N2 + j]*(uny[i*N2 + j]   - SiMAP[i*N2 + j])+
					A9[i*N2 + j]*(upxny[i*N2 + j] - SiMAP[i*N2 + j]));
		}
	}

	free(anx);   free(apx);   free(bnx);   free(bny);   free(bpx);   free(bpy);   free(cny);   free(cpy);
	free(A1);   free(A2);   free(A3);   free(A4);   free(A6);   free(A7);   free(A8);   free(A9);
	free(unx);   free(upx);   free(uny);   free(upy);   free(unxny);   free(upxpy); free(unxpy);   free(upxny);
}

void DRLSECore::Diffusion_scheme_2D_implicit(double *SiMAP, double *Dxx, double *Dxy, double *Dyy, unsigned int *imgSize, Options_p *Options)
{
	int i,j;
	int N1, N2, N3=1;
	unsigned int npixels2;
	double *Aiu, *Aid;
	double *Biu, *Bid, *Bil, *Bir;
	double *Cil, *Cir;
	double *Biuil, *Bidil, *Bidir, *Biuir;
	double *Dul,*Ddr,*Ddl,*Dur;
	double *Dml,*Dmr,*Duc,*Ddc;
	double *Dmc, *Du;
	double *Uiu, *Uid, *Uil, *Uir;
	double *Uiuil, *Uidil, *Uiuir, *Uidir;

	npixels2 = imgSize[0]*imgSize[1];
	N1 = imgSize[1];
	N2 = imgSize[0];
	Aiu=(double *)malloc(npixels2*sizeof(double));

	if (Aiu == NULL) printf("gradA: Out of Memory \n");

	Aid=(double *)malloc(npixels2*sizeof(double));

	if (Aid == NULL) printf("gradA: Out of Memory \n");

	Biu=(double *)malloc(npixels2*sizeof(double));

	if (Biu == NULL) printf("gradA: Out of Memory \n");

	Bid=(double *)malloc(npixels2*sizeof(double));

	if (Bid == NULL) printf("gradA: Out of Memory \n");

	Bil=(double *)malloc(npixels2*sizeof(double));

	if (Bil == NULL) printf("gradA: Out of Memory \n");

	Bir=(double *)malloc(npixels2*sizeof(double));

	if (Bir == NULL) printf("gradA: Out of Memory \n");

	Cil=(double *)malloc(npixels2*sizeof(double));

	if (Cil == NULL) printf("gradA: Out of Memory \n");

	Cir=(double *)malloc(npixels2*sizeof(double));

	if (Cir == NULL) printf("gradA: Out of Memory \n");

	Biuil=(double *)malloc(npixels2*sizeof(double));

	if (Biuil == NULL) printf("gradA: Out of Memory \n");

	Bidil=(double *)malloc(npixels2*sizeof(double));

	if (Bidil == NULL) printf("gradA: Out of Memory \n");

	Bidir=(double *)malloc(npixels2*sizeof(double));

	if (Bidir == NULL) printf("gradA: Out of Memory \n");

	Biuir=(double *)malloc(npixels2*sizeof(double));

	if (Biuir == NULL) printf("gradA: Out of Memory \n");

	Dul=(double *)malloc(npixels2*sizeof(double));

	if (Dul == NULL) printf("gradA: Out of Memory \n");

	Ddr=(double *)malloc(npixels2*sizeof(double));

	if (Ddr == NULL) printf("gradA: Out of Memory \n");

	Ddl=(double *)malloc(npixels2*sizeof(double));

	if (Ddl == NULL) printf("gradA: Out of Memory \n");

	Dur=(double *)malloc(npixels2*sizeof(double));

	if (Dur == NULL) printf("gradA: Out of Memory \n");

	Dml=(double *)malloc(npixels2*sizeof(double));

	if (Dml == NULL) printf("gradA: Out of Memory \n");

	Dmr=(double *)malloc(npixels2*sizeof(double));

	if (Dmr == NULL) printf("gradA: Out of Memory \n");

	Duc=(double *)malloc(npixels2*sizeof(double));

	if (Duc == NULL) printf("gradA: Out of Memory \n");

	Ddc=(double *)malloc(npixels2*sizeof(double));

	if (Ddc == NULL) printf("gradA: Out of Memory \n");

	Dmc=(double *)malloc(npixels2*sizeof(double));

	if (Dmc == NULL) printf("gradA: Out of Memory \n");

	Du=(double *)malloc(npixels2*sizeof(double));

	if (Du == NULL) printf("gradA: Out of Memory \n");

	for (i=0; i<N1 ;i++)
	{
		for (j=0; j<N2 ;j++)
		{
			if(i==0)
			{
				Aiu[j] = Dxx[j];
				Biu[j] = Dxy[j];
				Aid[j] = Dxx[N2 + j];
				Bid[j] = Dxy[N2 + j];
			}
			else
			{
				Aiu[N2*i + j] = Dxx[N2*(i-1) + j];
				Biu[N2*i + j] = Dxy[N2*(i-1) + j];
				Aid[N2*i + j] = Dxx[N2*(i+1) + j];
				Bid[N2*i + j] = Dxy[N2*(i+1) + j];
			}
		}
	}

	for (i=0; i<N2 ;i++)
	{
		for (j=0; j<N1 ;j++)
		{
			if(i==0)
			{
				Bil[N2*j] = Dxy[N2*j];
				Cil[N2*j] = Dyy[N2*j];
				Biuil[N2*j] = Biu[N2*j];                  //1 biuil: biu->bil
				Bidil[N2*j] = Bid[N2*j];
				Bir[N2*j] = Dxy[N2*j + 1];
				Cir[N2*j] = Dyy[N2*j + 1];
				Biuir[N2*j] = Biu[N2*j +1];
				Bidir[N2*j] = Bid[N2*j +1];
			}
			else
			{
				Bil[N2*j + i] = Dxy[N2*j + i-1];
				Cil[N2*j + i] = Dyy[N2*j + i-1];
				Biuil[N2*j + i] = Biu[N2*j + i-1];                  //1 biuil: biu->bil
				Bidil[N2*j + i] = Bid[N2*j + i-1];
				Bir[N2*j + i] = Dxy[N2*j + i+1];
				Cir[N2*j + i] = Dyy[N2*j + i+1];
				Bidir[N2*j + i] = Bid[N2*j + i+1];
				Biuir[N2*j + i] = Biu[N2*j + i+1];
			}
		}
	}

	for (i=0; i<N1 ; i++)
	{
		for (j=0; j<N2 ; j++)
		{
			Dul[N2*i+j] = max(0.0, Biuil[N2*i+j])  + max(0.0, Dxy[N2*i+j]);
			Ddr[N2*i+j] = max(0.0, Bidir[N2*i+j])  + max(0.0, Dxy[N2*i+j]);
			Ddl[N2*i+j] = max(0.0, -Bidil[N2*i+j]) + max(0.0, -Dxy[N2*i+j]);
			Dur[N2*i+j] = max(0.0, -Biuir[N2*i+j]) + max(0.0, -Dxy[N2*i+j]);
		}
	}

	// Normal neighbor intensity updates
	for (i=0; i<N1 ; i++)
	{
		for (j=0; j<N2 ; j++)
		{
			Dml[N2*i+j] = Cil[N2*i+j] + Dyy[N2*i+j] - (fabs(Bil[N2*i+j]) + fabs(Dxy[N2*i+j]));
			Dmr[N2*i+j] = Cir[N2*i+j] + Dyy[N2*i+j] - (fabs(Bir[N2*i+j]) + fabs(Dxy[N2*i+j]));
			Duc[N2*i+j] = Aiu[N2*i+j] + Dxx[N2*i+j] - (fabs(Biu[N2*i+j]) + fabs(Dxy[N2*i+j]));
			Ddc[N2*i+j] = Aid[N2*i+j] + Dxx[N2*i+j] - (fabs(Bid[N2*i+j]) + fabs(Dxy[N2*i+j]));
		}
	}

	// Normalization term to preserve region average grey value
	for (i=0; i<N1 ; i++)
	{
		for (j=0; j<N2 ; j++)
		{
			Dmc[N2*i+j] = -(Cil[N2*i+j] + 2*Dyy[N2*i+j] + Cir[N2*i+j])
					-(Aiu[N2*i+j] + 2*Dxx[N2*i+j] + Aid[N2*i+j])
					-(max(0.0, Biuil[N2*i+j]) + max(0.0, -Biuir[N2*i+j]))
					-(max(0.0, -Bidil[N2*i+j]) + max(0.0,  Bidir[N2*i+j]))
					+ fabs(Bil[N2*i+j]) + fabs(Bir[N2*i+j]) + fabs(Bid[N2*i+j]) + fabs(Biu[N2*i+j]) + 2.*fabs(Dxy[N2*i+j]);
		}
	}

	free(Aiu); free(Aid);
	free(Biu); free(Bid); free(Bil); free(Bir);
	free(Cil); free(Cir);
	free(Biuil); free(Bidil); free(Bidir); free(Biuir);

	Uiu=(double *)malloc(npixels2*sizeof(double));

	if (Uiu == NULL) printf("gradA: Out of Memory \n");

	Uid=(double *)malloc(npixels2*sizeof(double));

	if (Uid == NULL) printf("gradA: Out of Memory \n");

	Uil=(double *)malloc(npixels2*sizeof(double));

	if (Uil == NULL) printf("gradA: Out of Memory \n");

	Uir=(double *)malloc(npixels2*sizeof(double));

	if (Uir == NULL) printf("gradA: Out of Memory \n");

	Uiuil=(double *)malloc(npixels2*sizeof(double));

	if (Uiuil == NULL) printf("gradA: Out of Memory \n");

	Uidil=(double *)malloc(npixels2*sizeof(double));

	if (Uidil == NULL) printf("gradA: Out of Memory \n");

	Uiuir=(double *)malloc(npixels2*sizeof(double));

	if (Uiuir == NULL) printf("gradA: Out of Memory \n");

	Uidir=(double *)malloc(npixels2*sizeof(double));

	if (Uidir == NULL) printf("gradA: Out of Memory \n");

	for (i=0; i<N1 ;i++)
	{
		for (j=0; j<N2 ;j++)
		{
			if(i==0)
			{
				Uiu[j] = SiMAP[j];
				Uid[j] = SiMAP[N2 + j];
			}
			else
			{
				Uiu[N2*i + j] = SiMAP[N2*(i-1) + j];
				Uid[N2*i + j] = SiMAP[N2*(i+1) + j];
			}
		}
	}

	for (i=0; i<N2 ;i++)
	{
		for (j=0; j<N1 ;j++)
		{
			if(i==0)
			{
				Uil[N2*j] = SiMAP[N2*j];
				Uiuil[N2*j] = Uiu[N2*j];                  //1 biuil: biu->bil
				Uidil[N2*j] = Uid[N2*j];
				Uir[N2*j] = SiMAP[N2*j +1];
				Uidir[N2*j] = Uid[N2*j +1];
				Uiuir[N2*j] = Uiu[N2*j +1];
			}
			else
			{
				Uil[N2*j + i] = SiMAP[N2*j + i-1];
				Uiuil[N2*j + i] = Uiu[N2*j + i-1];                  //1 biuil: biu->bil
				Uidil[N2*j + i] = Uid[N2*j + i-1];
				Uir[N2*j + i] = SiMAP[N2*j + i+1];
				Uidir[N2*j + i] = Uid[N2*j + i+1];
				Uiuir[N2*j + i] = Uiu[N2*j + i+1];
			}
		}
	}

	//  Calculate Diffusion update
	// du=zeros(size(u));
	for (i=0; i<N1 ; i++)
	{
		for (j=0; j<N2 ; j++)
		{
			Du[N2*i+j] = 0.5*Options->dt*( Dul[N2*i+j]*Uiuil[N2*i+j] +
						 Dml[N2*i+j]*Uil[N2*i+j] +
						 Ddl[N2*i+j]*Uidil[N2*i+j] +
						 Duc[N2*i+j]*Uiu[N2*i+j] +
						 Ddc[N2*i+j]*Uid[N2*i+j] +
						 Dur[N2*i+j]*Uiuir[N2*i+j] +
						 Dmr[N2*i+j]*Uir[N2*i+j] +
						 Ddr[N2*i+j]*Uidir[N2*i+j]);
		}
	}

	free(Uiu); free(Uid); free(Uil); free(Uir);
	free(Uiuil); free(Uidil); free(Uiuir); free(Uidir);
	free(Dul); free(Ddr); free(Ddl); free(Dur);
	free(Dml); free(Dmr); free(Duc); free(Ddc);

	// Perform the edge preserving diffusion filtering on the image
	for (i=0; i<N1 ; i++)
	{
		for (j=0; j<N2 ; j++)
		{
			SiMAP[i*N2 + j] = (SiMAP[i*N2 + j] + Du[i*N2 + j])/(1.0 - Options->dt*0.5*Dmc[N2*i+j]);
		}
	}

	free(Dmc); free(Du);
}

void DRLSECore::drlse_edge(double *Phi, double *g, double lambda, double mu, double alfa, double epsilon, double timestep, int iter_inner, char potentialFunction, unsigned int *imgSize)
{
	unsigned int npixels2;
	int i,j,n;                                 // iter -> iter_inner;
	double smallNumber, S;
	double *Vx, *Vy, *Phix, *Phiy, *Nx, *Ny;
	double *curvature;
	double *diracPhi, areaTerm, edgeTerm, *distRegTerm;

	npixels2 = imgSize[0]*imgSize[1];
	Vx = (double*)malloc(npixels2*sizeof(double));
	Vy = (double*)malloc(npixels2*sizeof(double));
	Phix = (double*)malloc(npixels2*sizeof(double));
	Phiy = (double*)malloc(npixels2*sizeof(double));
	Nx = (double*)malloc(npixels2*sizeof(double));
	Ny = (double*)malloc(npixels2*sizeof(double));
	curvature = (double*)malloc(npixels2*sizeof(double));
	diracPhi = (double*)malloc(npixels2*sizeof(double));
	distRegTerm = (double*)malloc(npixels2*sizeof(double));

	printf("55 DRLSE: Unknown option\n" );

	// Smooth image by Gaussiin convolution
	GradientM(g, imgSize, Vx, Vy);

	for (n=1; n <=iter_inner ; n++)       //  iter_inner
	{
		NeumannBoundCond(Phi, imgSize);
		GradientM(Phi, imgSize, Phix, Phiy);

		for (j=0; j<npixels2; j++)
		{
			S=sqrt(pow(Phix[j], 2.0) + pow(Phiy[j], 2.0));     // Matlab : s=sqrt(phi_x.^2 + phi_y.^2);
			smallNumber = 1e-10;
			//  Add a small positive number to avoid division by zero
			Nx[j] = Phix[j]/(S+smallNumber);                   // Matlab : Nx=phi_x./(s+smallNumber);
			Ny[j] = Phiy[j]/(S+smallNumber);                   // Matlab : Ny=phi_y./(s+smallNumber);
		}

		curvature = divergence(Nx,Ny, imgSize);                                // Matlab : curvature=div(Nx,Ny);
		potentialFunction ='y';

		if (potentialFunction == 'x'){}     // Singel-well
		else if (potentialFunction == 'y')  // Double-well
		{
			distRegTerm=distReg_p2(Phi, imgSize);    // Compute the distance regularization term in eqaution (13) with the double-well potential p2.
		}
		else
		{
			printf("Error: Wrong choice of potential function. Please input the string 'single-well' or 'double-well' in the drlse_edge function. \n");
		}

		diracPhi=Dirac(Phi, epsilon, imgSize);

		for (i=0; i<npixels2; i++)
		{
			areaTerm=diracPhi[i]*g[i];           // areaTerm=diracPhi.*g;   balloon/pressure force
			edgeTerm=diracPhi[i]*((Vx[i]*Nx[i]+Vy[i]*Ny[i]) + g[i]*curvature[i]);   // edgeTerm=diracPhi.*(vx.*Nx+vy.*Ny) + diracPhi.*g.*curvature;
			Phi[i] = Phi[i] + timestep*(mu*distRegTerm[i] + lambda*edgeTerm + alfa*areaTerm); // phi = phi + timestep*(mu*distRegTerm + lambda*edgeTerm + alfa*areaTerm);
		}
	}  // (i=1; i <= iter_inner; i++)

	free(Vx);
	free(Vy);
	free(Phix);
	free(Phiy);
	free(Nx);
	free(Ny);
	free(curvature);
	free(diracPhi);
	free(distRegTerm);
}

void DRLSECore::contour(double *iMAP_1D, double *Phi, unsigned int *imgSize)
{
	FILE *out;
	unsigned int npixels2;
	double *line, contourline;
	int cWidth, rHeight, i, j, n,m, skip, flag;

	cWidth = imgSize[0];
	rHeight = imgSize[1];;
	npixels2= cWidth*rHeight;
	line = (double*)malloc(npixels2*sizeof(double));
	out = fopen ("contour.dat","w");

	for (i=1; i< (rHeight-1); i++)
	{
		for (j=1; j< (cWidth-1); j++)
		{
			flag = 0;
			line[i*cWidth + j] = Phi[i*cWidth + j];

			for (n= -1; n <= 1; n++)
			{
				if (n==0) skip = 2;
				else skip =1;

				for (m=-1; m <= 1; m=m+skip)
				{
					contourline = Phi[(i+n)*cWidth + (j+m)]*Phi[i*cWidth + j];

					if (contourline < 0.0 && flag == 0)
					{
						line[i*cWidth + j] = 0.93;
						iMAP_1D[i*cWidth + j] =200;
						flag = 1;
					}
				}

			}  // for (n= -1; n <= 1; n++){
		}
	}

	for (i=0; i< npixels2; i++)
	{
		fprintf(out, "%f \n",  line[i]);
	}

	free(line);
	fclose(out);
}

void DRLSECore::NeumannBoundCond(double *Phi, unsigned int *imgSize)
{
	unsigned int npixels2;
	int i,j;
	int nrow, ncol;

	npixels2 = imgSize[0]*imgSize[1];              // imgSize[0] : width (x-axis, column), imgSize[1] : height (y-axis, row)
	nrow = imgSize[1];
	ncol = imgSize[0];
	Phi[0*ncol+0] = Phi[2*ncol+2];                             // Matlab : g([1 nrow],[1 ncol]) = g([3 nrow-2],[3 ncol-2]);
	Phi[(nrow-1)*ncol+0] = Phi[(nrow-3)*ncol+2];               // Matlab : g([1 nrow],[1 ncol]) = g([3 nrow-2],[3 ncol-2]);
	Phi[0*ncol+(ncol-1)] = Phi[2*ncol+(ncol-3)];               // Matlab : g([1 nrow],[1 ncol]) = g([3 nrow-2],[3 ncol-2]);
	Phi[(nrow-1)*ncol+(ncol-1)] = Phi[(nrow-3)*ncol+(ncol-3)]; // Matlab : g([1 nrow],[1 ncol]) = g([3 nrow-2],[3 ncol-2]);

	for (j=1; j<(ncol-1) ; j++)
	{
		 Phi[0*ncol+j] = Phi[2*ncol+j];                         // Matlab : g([1 nrow],2:end-1) = g([3 nrow-2],2:end-1);
		 Phi[(nrow-1)*ncol+j] = Phi[(nrow-3)*ncol+j];            // Matlab : g([1 nrow],2:end-1) = g([3 nrow-2],2:end-1);
	}

	for (i=1; i<(nrow-1) ; i++)
	{
		 Phi[i*ncol+0] = Phi[i*ncol+2];                         // Matlab : g(2:end-1,[1 ncol]) = g(2:end-1,[3 ncol-2])
		 Phi[i*ncol+(ncol-1)] = Phi[i*ncol+(ncol-3)];           // Matlab : g(2:end-1,[1 ncol]) = g(2:end-1,[3 ncol-2])
	}
}

double *DRLSECore::divergence(double *Nx, double *Ny, unsigned int *imgSize)
{
	unsigned int npixels2;
	int i;
	double *Nx0, *Ny0, *f;

	npixels2 = imgSize[0]*imgSize[1];              // imgSize[0] : width (x-axis, column), imgSize[1] : height (y-axis, row)
	Nx0 = (double*)malloc(npixels2*sizeof(double));
	Ny0 = (double*)malloc(npixels2*sizeof(double));
	f = (double*)malloc(npixels2*sizeof(double));
	GradientM(Nx, imgSize, Nx0, Ny0);

	for(i=0; i<npixels2; i++)
	{
		f[i] = Nx0[i];
	}

	GradientM(Ny, imgSize, Nx0, Ny0);

	for(i=0; i<npixels2; i++)
	{
		f[i] = f[i] + Ny0[i];
	}

	free(Nx0);
	free(Ny0);

	return f;
}

double *DRLSECore::distReg_p2(double *Phi, unsigned int *imgSize)
{
	unsigned int npixels2;
	int i;
	double *Phix, *Phiy;
	double a,b, S, ps, psN, psY, sN, sY, dps;
	double *q, *dpsPhix, *dpsPhiy, *Lap, *divG ;

	npixels2 = imgSize[0]*imgSize[1];              // imgSize[0] : width (x-axis, column), imgSize[1] : height (y-axis, row)
	Phix = (double*)malloc(npixels2*sizeof(double));
	Phiy = (double*)malloc(npixels2*sizeof(double));
	dpsPhix = (double*)malloc(npixels2*sizeof(double));
	dpsPhiy = (double*)malloc(npixels2*sizeof(double));
	Lap  = (double*)malloc(npixels2*sizeof(double));
	divG  = (double*)malloc(npixels2*sizeof(double));
	q = (double*)malloc(npixels2*sizeof(double));
	GradientM(Phi, imgSize, Phix, Phiy);

	for (i=0; i<npixels2; i++)
	{
		S=sqrt(pow(Phix[i], 2.0) + pow(Phiy[i], 2.0));     // Matlab : s=sqrt(phi_x.^2 + phi_y.^2);

		if ((S >= 0.0) && (S <= 1.0))      // Matlab: a=(s>=0) & (s<=1);
		{
			a = 1.0;
		}
		else
		{
			a = 0.0;
		}

		if (S > 1.0)                      // Matlab: b=(s>1);
		{
			b = 1.0;
		}
		else
		{
			b = 0.0;
		}

		ps  = a*sin(2.*PI*S)/(2*PI)+ b*(S-1.0);  // Compute first order derivative of the double-well potential p2 in eqaution (16)

		if (ps != 0.0)
		{
			psN = 1.0;
			psY = 0.0;
		}
		else
		{
			psN = 0.0;
			psY = 1.0;
		}

		if (S != 0.0)
		{
			sN = 1.0;
			sY = 0.0;
		}
		else
		{
			sN = 0.0;
			sY = 1.0;
		}

		dps = (psN*ps + psY)/(sN*S + sY); // Compute d_p(s)=p'(s)/s in equation (10). As s-->0, we have d_p(s)-->1 according to equation (18)
		dpsPhix[i] =  (dps-1.0)*Phix[i];
		dpsPhiy[i] =  (dps-1.0)*Phiy[i];

	}  // for (i=0; i<npixels2; i++){

	divG = divergence(dpsPhix, dpsPhiy, imgSize);
	del2(Phi, Lap, imgSize);

	for (i=0; i<npixels2; i++)
	{
		q[i] = divG[i] + 4.0*Lap[i];   // Matlab: f = div(dps.*phi_x - phi_x, dps.*phi_y - phi_y) + 4*del2(phi);
	}

	free(Phix);
	free(Phiy);
	free(dpsPhix);
	free(dpsPhiy);
	free(Lap);
	free(divG);

	return q;
}

void DRLSECore::del2(double *Phi, double *Lap, unsigned int *imgSize)
{
	unsigned int npixels2;
	int i, j;
	int width = imgSize[0];   //img.GetWidth();
	int height = imgSize[1];    // img.GetHeight();

	npixels2 = imgSize[0]*imgSize[1];              // imgSize[0] : width (x-axis, column), imgSize[1] : height (y-axis, row)

	for( i = 0 ; i < height ; i++ )
	{
		Lap[i*width + 0] = Phi[i*width + 1]*2. - Phi[i*width + 2];
		Lap[i*width + (width -1)]    = Phi[i*width + width-2]*2. - Phi[i*width + width-3];
	}

	for( i = 0 ; i < height   ; i++ )
	{
		for( j = 1 ; j < width-1 ; j++ )
		{
			Lap[i*width + j] = ((Phi[i*width + (j+1)] - Phi[i*width + j]) - (Phi[i*width + j] - Phi[i*width + (j-1)]))/2.;
		}
	}

	/* y - direction */
	for( j = 0; j < width ; j++ )
	{
		Lap[0*width + j] += Phi[1*width + j]*2. - Phi[2*width + j];
		Lap[(height-1)*width + j] += Phi[(height-2)*width + j]*2. - Phi[(height-3)*width + j];

	}

	for( i = 1 ; i < (height-1) ; i++ )
	{
		for( j = 0 ; j < width   ; j++ )
		{
			Lap[i*width + j] += ((Phi[(i+1)*width + j] - Phi[i*width + j]) - (Phi[i*width + j] - Phi[(i-1)*width + j]))/2.;
		}
	}

	for( i = 0 ; i < height ; i++ )
	{
		for( j = 0 ; j < width ; j++ )
		{
			Lap[i*width + j] /= 2.;
		}
	}
}

double *DRLSECore::Dirac(double *Phi, double epsilon, unsigned int *imgSize)
{
	unsigned int npixels2;
	int i;
	double *f, b;

	npixels2 = imgSize[0]*imgSize[1];              // imgSize[0] : width (x-axis, column), imgSize[1] : height (y-axis, row)
	f = (double*)malloc(npixels2*sizeof(double));

	for(i=0; i<npixels2; i++)
	{
		f[i] = (0.5/epsilon)*(1.+cos(PI*Phi[i]/epsilon));          // Matlab: f=(1/2/sigma)*(1+cos(pi*x/sigma)); cos(PI*Phi[i]/epsilon)

		if ((Phi[i] >= -epsilon) && (Phi[i] <=epsilon))             // Matlab: b = (x<=sigma) & (x>=-sigma);
		{
			b = 1.0;
		}
		else
		{
			b = 0.0;
		}

		f[i] = f[i]*b;                               // Matlab:  f = f.*b;
	}

	return f;
}

void DRLSECore::GradientM(double *SiMAP, unsigned int *imgSize, double *Ux, double *Uy)
{
	unsigned int npixels2;
	int i, j;
	int width  = imgSize[0];    // img.GetWidth();
	int height = imgSize[1];     // img.GetHeight();

	npixels2 =  imgSize[0]*imgSize[1];              // imgSize[0] : width (x-axis, column), imgSize[1] : height (y-axis, row)

	for ( i = 0 ; i < height; i++ )
	{
		Ux[i*width + 0] =  SiMAP[i*width + 1] - SiMAP[i*width + 0];
		Ux[i*width + width-1] =  SiMAP[i*width + width-1] - SiMAP[i*width + width-2];
	}

	for ( i = 0 ; i < height   ; i++ )
	{
		for ( j = 1 ; j < width - 1 ; j++ )
		{
			Ux[i*width + j] =  (SiMAP[i*width + j+1] - SiMAP[i*width + j-1])/2.;
		}
	}

	for( j = 0 ; j < width ; j++ )
	{
		Uy[0 + j] =  SiMAP[1*width + j] - SiMAP[0 + j];
		Uy[(height-1)*width + j] =  SiMAP[(height-1)*width + j] - SiMAP[(height-2)*width + j];
	}

	for ( i = 1 ; i < height-1 ; i++ )
	{
		for ( j = 0 ; j < width ; j++ )
		{
			Uy[i*width + j] =  (SiMAP[(i+1)*width + j] - SiMAP[(i-1)*width + j])/2.;
		}
	}
}

void DRLSECore::GaussianFiltering2D_double(double *iMAP_1D, unsigned int *imgSize, double sigma, int kernel_size, double *SiMAP)
{
	int i;
	int kernel_length;
	double x, *H, totalH=0;

	/* Construct the 1D gaussian kernel */
	if(kernel_size<1) kernel_size=1;

	kernel_length=(int)(2*ceil(kernel_size/2)+1);
	H = (double *)malloc(kernel_length*sizeof(double));
	x=-ceil(kernel_size/2);

	for (i=0; i<kernel_length; i++)
	{
		H[i]=exp(-((x*x)/(2*(sigma*sigma)))); totalH+=H[i]; x++;
	}

	for (i=0; i<kernel_length; i++)
	{
		H[i]/=totalH;
	}

	/* Do the filtering */
	imfilter2D_double(iMAP_1D, imgSize, &H[0], kernel_length, &SiMAP[0]);

   /* Clear memory gaussian kernel */
	free(H);
}
